/**
 * @file optitopo.h
 * @author mzy
 * @brief optimize the topology of swarm robot
 * @date 2022-01-18
 */

#ifndef OPTOTOPO_H
#define OPTOTOPO_H

#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <algorithm>
#include <iostream>
#include <complex.h>
#include <time.h>

using namespace Eigen;

// 寻找最大fitness，注释后则寻找最小fitness
#define MAXIMIZE_FITNESS

struct Particle
{
	int dim_;							// 参数维度（position和velocity的维度）
	double fitness_;
	double *position_ = nullptr;
	double *velocity_ = nullptr;

	double *best_position_ = nullptr;
	double best_fitness_;
	double *results_ = nullptr;			// 一些需要保存出的结果
	int results_dim_ = 0;				// results_的维度

	Particle(){}

	~Particle()
	{
		if (position_) { delete[]position_; }
		if (velocity_) { delete[]velocity_; }
		if (best_position_) { delete[]best_position_; }
		if (results_) { delete[]results_; }
	}
};


class optitopo {
    public:
        //规划参数
        double T_exp_;//无人机的期望的运动周期，单位：s
        double k_v_theta_max_;// 允许无人机最大运动角速度系数
        double k_v_theta_min_;// 允许无人机最大运动角速度系数
        double a_theta_max_;//无人机最大运动角加速度，单位：rad/s^2
        double dis_min_; //两架无人机之间最小距离，单位：m
        int Resolution_; //离散点的数量
        double Coeff_Vel_;//速度权重系数
        double Delta_t_; //每次搜索的时间间隔
        double v_theta_exp_;

        double lambda2;                     //拉普拉斯矩阵的次小特征值
        double TraceofL;                    //拉普拉斯矩阵的迹
        double SeeWho;                      //每一架无人机看谁
        Vector4d alpha_out;
        Vector4d d_alpha_out;
        Vector4d theta_out;
        Vector4d d_theta_out;
        Vector4d dd_theta_out;
        Vector4d alpha_in_;
        Vector4d theta_in_;
        Vector4d d_theta_in_;
        Vector4d pre_theta_;
        Vector4d pre_alpha_;


        //粒子优化函数
        int particle_num_;					// 粒子个数
        int max_iter_num_;					// 最大迭代次数
        int curr_iter_;						// 当前迭代次数
        int dim_;							// 参数维度（position和velocity的维度）
        Particle *particles_ = nullptr;		// 所有粒子

        double *upper_bound_ = nullptr;					// position搜索范围上限
        double *lower_bound_ = nullptr;					// position搜索范围下限
        double *range_interval_ = nullptr;				// position搜索区间长度

        double *dt_ = nullptr;							// 时间步长
        double *wstart_ = nullptr;						// 初始权重
        double *wend_ = nullptr;						// 终止权重
        double *w_ = nullptr;							// 当前迭代权重
        double *C1_ = nullptr;							// 加速度因子
        double *C2_ = nullptr;							// 加速度因子

        double all_best_fitness_;						// 全局最优粒子的适应度值
        double *all_best_position_ = nullptr;			// 全局最优粒子的poistion
        double *results_ = nullptr;						// 一些需要保存出的结果
        int results_dim_ = 0;							// results的维度
        bool Iter_Type_ = 0;                           // 迭代分为两个阶段，0：迭代相机角度，1：迭代无人机位置

    public:
        // 默认构造函数
	    optitopo() {}
        // 构造函数
        optitopo(int dim, int particle_num, int results_dim);
        // 析构函数
        ~optitopo();

        double GetWeight(double theta_i, double theta_j, double alpha_i);
        bool GetLambda2(std::vector<double> theta, std::vector<double> alpha);
        double SolveLambda2_Analytical(Matrix4d Laplace);
        double SolveLambda2_Eigen(Matrix4d Laplace);
        double SolveLambda2_PowerIteration(Matrix4d Laplace, int iter_times);

        // 利用暴力搜索 搜索最终结果
        bool GetOutput_search(std::vector<double> theta, std::vector<double> d_theta, std::vector<double> alpha, double Delta_t);
        // 利用粒子群算法 搜索最终结果
        bool GetOutput_pso(std::vector<double> theta, std::vector<double> d_theta, std::vector<double> alpha, double Delta_t);

        // PSO function
        // 初始化所有粒子参数
        void InitialAllParticles();
        // 初始化第i个粒子参数
        void InitialParticle(int i);
        // 获取双精度随机数（默认精度为0.0001）
        double GetDoubleRand(int N = 9999);
        // 计算该粒子的适应度值
        double GetFitness_Camera(Particle& particle);
        double GetFitness_Uav(Particle& particle);
        // 完成设定次数的迭代过程
        void GetFinalResult();
        // 更新所有粒子参数
        void UpdateAllParticles();
        // 更新第i个粒子
        void UpdateParticle(int i);
        // 获取当前迭代的权重
        void GetInertialWeight();
        
};

#endif //OPTOTOPO_H